#include "qmc5883/qmc5883.h"
#include "stdio.h"
#include "schedule.h"
#include "main.h"
#include "vtor_i2c.h"


char	qmc5883Address = (0x1a);


struct Qmc5883Data qmcData = {0};
unsigned char qmcBuffer[10] = {0};

extern  I2C_HandleTypeDef hi2c1;
extern  I2C_HandleTypeDef hi2c2;

I2C_HandleTypeDef* hmcI2C = &hi2c1;
extern struct VtorI2c si2c2;


struct VtorI2c* qmcvi2c = &si2c2;



void Qmc5883_WriteReg(uint8_t regAddr, uint8_t* dataBuf, uint8_t dataLen)
{
	VtorI2c_WriteMem(qmcvi2c, qmc5883Address, regAddr, 1, dataBuf, dataLen);
}
void Qmc5883_ReadReg(uint8_t regAddr, uint8_t* dataBuf, uint8_t dataLen)
{
	VtorI2c_ReadMem(qmcvi2c, qmc5883Address, regAddr, 1, dataBuf, dataLen);
}


unsigned char Qmc5883_GetData(void)
{
	char ret = 0;
    unsigned char regData;
	Qmc5883_ReadReg(0x06, &regData, 1);
	ret <<= 1;
	if (regData & 0x01)
	{
		Qmc5883_ReadReg(0x0, qmcBuffer, 6);
		ret <<= 1;
		Qmc5883_Translate(&qmcData, qmcBuffer);
		return ret;
	}

    return ret;
}
void Qmc5883_Calib()
{
	short calibTime = 5000;
	while(calibTime --)
	{
		Qmc5883_GetData();
		if (qmcData.x < qmcData.xMin)
		{
			qmcData.xMin = qmcData.x;
		}
		else if (qmcData.x > qmcData.xMax)
		{
			qmcData.xMax = qmcData.x;
		}
		if (qmcData.y < qmcData.yMin)
		{
			qmcData.yMin = qmcData.y;
		}
		else if (qmcData.y > qmcData.yMax)
		{
			qmcData.yMax = qmcData.y;
		}
		if (qmcData.z < qmcData.zMin)
		{
			qmcData.zMin = qmcData.z;
		}
		else if (qmcData.z > qmcData.zMax)
		{
			qmcData.zMax = qmcData.z;
		}
		HAL_Delay(1);
	}
	qmcData.xOffest = (qmcData.xMax+qmcData.xMin)/2;
	qmcData.yOffest = (qmcData.yMax+qmcData.yMin)/2;
	qmcData.xK = 2/(qmcData.xMax-qmcData.xMin);
	qmcData.yK = 2/(qmcData.xMax-qmcData.xMin);

	printf("x %d %d y %d %d\r\n",
			qmcData.xOffest,qmcData.xK,
			qmcData.yOffest,qmcData.yK);
}

void Qmc5883_Translate(struct Qmc5883Data *qmcData, unsigned char *qmcBuffer)
{
	qmcData->x = qmcBuffer[0] | qmcBuffer[1] << 8;
	qmcData->y = qmcBuffer[2] | qmcBuffer[3] << 8;
	qmcData->z = qmcBuffer[4] | qmcBuffer[5] << 8;
	qmcData->temp = qmcBuffer[7] |  qmcBuffer[8] << 8;

	qmcData->x = (qmcData->x - qmcData->xOffest) * qmcData->xK;
	qmcData->y = (qmcData->y - qmcData->yOffest) * qmcData->yK;

	qmcData->roll = atan2((double)qmcData->y, (double)qmcData->x) * 57.3 + 180;
}

//��ʼ��HMC5883��������Ҫ��ο�pdf�����޸�****
void Qmc5883_Init()
{
	unsigned char retData = 0;
	// test
						//0x0d = 00 00    11 01
	retData = 0x01;
	Qmc5883_WriteReg(0x0b,&retData,0x01);
	Qmc5883_ReadReg(0x0b,&retData,1);
	printf("0x%x\r\n", retData);

	retData = 0x40;
	Qmc5883_WriteReg(0x20,&retData,1);
	Qmc5883_ReadReg(0x20,&retData,1);
	printf("0x%x\r\n", retData);

	retData = 1;
	Qmc5883_WriteReg(0x21,&retData,1);
	Qmc5883_ReadReg(0x21,&retData,1);
	printf("0x%x\r\n", retData);

	retData = 0x0d;
	Qmc5883_WriteReg(0x09,&retData,1);
	Qmc5883_ReadReg(0x09,&retData,1);
	printf("0x%x\r\n", retData);
}
